#ifndef _HESAI_PARSER_H_
#define _HESAI_PARSER_H_
#include <cmath>

#include "modules/common_msgs/sensor_msgs/pointcloud.pb.h"

#include "modules/drivers/lidar/orin_hesai/common/data_type.h"
#include "modules/drivers/lidar/orin_hesai/common/make_time.h"
#include "modules/drivers/lidar/orin_hesai/hesai_utils/hesai_data_type.h"

namespace apollo {
namespace drivers {
namespace lidar_hesai {
using apollo::drivers::PointCloud;
using apollo::drivers::PointXYZIT;
class HesaiParser {
 public:
  bool Init();
  bool ParseData(const HesaiLidarPacketData *data, PointCloud *cloud,
                 uint32_t &startAzimuth, uint32_t &endAzimuth);
  static int AppendZeroPacketByAzimuthGap(const uint32_t azimuthGap,
                                          PointCloud *cloud, bool flag = true);
  static void AppendZeroPoint(const uint32_t num, PointCloud *cloud);

 private:
  void InitHoriCosSinTable();
  void InitVertCosSinTable();
  void InitBlockOffset();
  void InitLaserOffset();

  bool Unpack(const HesaiLidarPacketData &buf, HS_LIDAR_XT_Packet &packet);
  bool CalcPointCloud(HS_LIDAR_XT_Packet &packet, PointCloud *cloud,
                      uint32_t &startAzimuth, uint32_t &endAzimuth);
  void CalcPointXYZIT(HS_LIDAR_XT_Packet &pkt, int blockid, PointCloud *cloud);
  inline double DegreeToRadian(double degree) { return degree * M_PI / 180; }

 private:
  double sin_hori_table_[36001][UNIT_PER_BLOCK];
  double cos_hori_table_[36001][UNIT_PER_BLOCK];
  double sin_vert_table_[UNIT_PER_BLOCK];
  double cos_vert_table_[UNIT_PER_BLOCK];
  double block_offset_[BLOCK_NUM];
  double laser_offset_[UNIT_PER_BLOCK];
  HS_LIDAR_XT_Packet packet_buffer_;
  MakeTime *const make_time_{MakeTime::Instance()};
};
}  // namespace lidar_hesai
}  // namespace drivers
}  // namespace apollo
#endif
